#include "MBCameraModel.h"

using std::vector;

/**
  *	Constructs a new MBCameraModel object.
  *
  * @param intrinsicParams intrinsic parameters of the camera model
  * @param internalCalibrationParams internal calibration parameters
  * @param distortionCoefsTopLeft true when the distortion coefficients refer to the image origin in the top left corner, false otherwise
  */
MBCameraModel::MBCameraModel(
	const MBCameraModelIntrinsicParameters &intrinsicParams,
	const MBCameraModelInternalCalibrationParameters &internalCalibrationParams,
	const bool &imageOriginTopLeft,
	const MBuint rieglVersion ) :
		mIntrinsicParams( intrinsicParams ),
		mInternalCalibrationParams( internalCalibrationParams ),
		mIsImageOriginTopLeft( imageOriginTopLeft ),
		mCameraMatrix( internalCalibrationParams.mFocalLengthXInPixels, 0.0, internalCalibrationParams.mCenterOfImageXInPixels,
					   0.0, internalCalibrationParams.mFocalLengthYInPixels, internalCalibrationParams.mCenterOfImageYInPixels,
					   0.0, 0.0, 1.0 ),
		mNonOrthogonality( internalCalibrationParams.mNonOrthogonality ),
		mDifferentialScale( internalCalibrationParams.mDifferentialScale ),
		mRieglVersion( rieglVersion )
{
	const MBuint NUM_COEFS = 8u;
	double distortion_coefs[] = {
		internalCalibrationParams.mRadialDistortionParamK1,
		internalCalibrationParams.mRadialDistortionParamK2,
		internalCalibrationParams.mTangentialDistortionParamP1,
		internalCalibrationParams.mTangentialDistortionParamP2,
		internalCalibrationParams.mRadialDistortionParamK3,
		internalCalibrationParams.mRadialDistortionParamK4,
		0.0,
		0.0,
		0.0 };
	mDistortionCoefs.assign( &distortion_coefs[0], &distortion_coefs[NUM_COEFS] );
}

const bool &MBCameraModel::IsImageOriginTopLeft( void ) const
{
	return mIsImageOriginTopLeft;
}

/**
  *	Returns a const reference to the camera matrix
  *
  * @return const reference to the camera matrix
  */
const MBMatrix3x3 &MBCameraModel::GetCameraMatrix( void ) const
{
	return mCameraMatrix;
}

const double &MBCameraModel::GetDifferentialScale( void ) const
{
	return mDifferentialScale;
}

/**
  *	Returns a const reference to the std::vector containing the distortion coefficients
  *
  * @return const reference to the std::vector containing the distortion coefficients
  */
const vector<double> &MBCameraModel::GetDistortionCoefs( void ) const
{
	return mDistortionCoefs;
}

/**
  *	Returns a const reference to the MBCameraModelInternalCalibrationParameters object,
  * that is encapsulating the internal calibration parameters
  *
  * @return const reference to the MBCameraModelInternalCalibrationParameters object, that is encapsulating the internal calibration parameters
  */
const MBCameraModelInternalCalibrationParameters &MBCameraModel::GetInternalCalibrationParameters( void ) const
{
	return mInternalCalibrationParams;
}

/**
  *	Returns a const reference to the MBCameraModelIntrinsicParameters object,
  * that is encapsulating the intrinsic parameters
  *
  * @return const reference to the MBCameraModelIntrinsicParameters object, that is encapsulating the intrinsic parameters
  */
const MBCameraModelIntrinsicParameters &MBCameraModel::GetIntrinsicParameters( void ) const
{
	return mIntrinsicParams;
}

/**
  *	Returns the Non-Orthogonality paramter of the camera model
  *
  * @return Non-Orthogonality paramter of the camera model
  */
const double &MBCameraModel::GetNonOrthogonality( void ) const
{
	return mNonOrthogonality;
}

/**
  *	Returns 0 if r^2 = x^2 + y^2, returns 1 if r^2 = arctan(sqrt(x^2 + y^2))
  * This parameter is only needed for RiScan Pro projects
  *
  * @return 0 if r^2 = x^2 + y^2, returns 1 if r^2 = arctan(sqrt(x^2 + y^2))
  */
const MBuint &MBCameraModel::GetRieglVersion( void ) const
{
	return mRieglVersion;
}

/**
  *	Returns true when the current MBCameraModel object is equal to the passed one, false otherwise
  *
  * @param other MBCameraModel object that is compared to the current one
  * @return true when the current MBCameraModel object is equal to the passed one, false otherwise
  */
bool MBCameraModel::operator==( const MBCameraModel &other ) const
{
	return (mIntrinsicParams == other.mIntrinsicParams &&
	mInternalCalibrationParams == other.mInternalCalibrationParams);
}